#include <stdlib.h>
#include <stdio.h>
#include <pthread.h>
#include <unistd.h>
#include <signal.h>
#include <semaphore.h>
#include <math.h>
#include <string.h>
#include <float.h>
#include <sys/time.h>
#include <GL/glut.h>
#include <GL/freeglut.h>
#include <netpbm/pam.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cblas.h>

#include "kf_matricies.h"
#include "bzrc.h"
#include "matrix.h"

#define IP_ADDRESS "127.0.0.1"
#define FLAG_RADIUS 3.0f

#define SPEED_SCALAR 1.8f

#define FLAG_STEP 0.7f // this is alpha
#define OBSTACLE_STEP 0.65f // this is beta
#define TANGENT_STEP 0.01f // beta, but for tangent field

#define FLAG_SPREAD 40.0f
#define OBSTACLE_SPREAD 60.0f
#define TANGENT_SPREAD 0.1f

// Numerical Constants
#define PI 3.14159265358979323846
#define F_INFINITY 50

#define WORLD_UPDATE_RATE 200000
#define AGENT_SLEEP_TIME 500000

#define SENSOR_NOISE 5

// Globals
extern int debug;
int running;
sem_t mutex;

typedef struct {
	FILE* fSocket;
	flag* flags;
	int flagCount;
	tank* tanks;
	int tankCount;
	enemyTank* enemyTanks;
	int enemyTankCount;
	base* bases;
	int baseCount;
        obstacle* obstacles;
        int obstacleCount;
	constant* constants;
	int constantCount;
} world_info;

typedef struct {
	FILE* fSocket;
	int tankID;
	world_info* world;
        int flagGoalID;
} param;

// Thread functions
void *driver(void* args);
void *updater(void* args);
void *visualizer(void* args);
void *gunner(void* args);

// Potential field functions
pair seekGoal(float x, float y, float xg, float yg, float radiusg, float* retSpeed);
float move(FILE* fSocket, int tankID, float angle, float previousAngle, pair delta, float speed, float timeDiff);

// Helper functions
void intHandler();
float getDistanceBetween(float x, float y, float xg, float yg);
float getAngleBetween(float x, float y, float xg, float yg);
float normalizeAngle(float angle);
int timeval_subtract(struct timeval *result, struct timeval *t2, struct timeval *t1);
void plot(char* filename, gsl_matrix* u, gsl_matrix* sigma);

// Matrix functions
gsl_matrix* createFMatrix(float c);


// Kalman Filter Functions
void calcKalmanGainAtTPlus1(
        gsl_matrix* f,
	gsl_matrix* h,
	gsl_matrix* hTrans,
	gsl_matrix* sigma_t,
	gsl_matrix* sigma_x,
	gsl_matrix* sigma_z,
	gsl_matrix* k);

void calcMeanAtTPlus1(
	gsl_matrix* f,
	gsl_matrix* h,
        gsl_matrix* k,
	gsl_matrix* z,
	gsl_matrix* ut,
	gsl_matrix* ut1);

void calcVarianceAtTPlus1(
	gsl_matrix* f,
	gsl_matrix* h,
	gsl_matrix* sigma_t,
	gsl_matrix* sigma_x,
        gsl_matrix* k,
	gsl_matrix* i,
	gsl_matrix* sigma_t1);

void updateFMatrix(gsl_matrix* matrix, double deltaT);


int main(int argc, char* argv[]) {
	int tcpSocket = 0;
	FILE* fSocket = 0;
	pthread_t* threads;
	pthread_t worldThread;
	pthread_t visualizerThread;
	param* threadArgs;
	world_info world;
	int threadCount;
	int i;

	running = 1;
	signal(SIGINT, intHandler);
	signal(SIGKILL, intHandler);
	sem_init(&mutex, 0, 0);

	if (argc < 2) {
		fprintf(stderr, "usage: gf_agent numagents port [debug 1:0]\n");
		exit(-1);
	}
	if (argc == 4 && atoi(argv[3]) == 1) {
		debug = 1;
	}

	threadCount = atoi(argv[1]);
	threads = (pthread_t*)malloc(sizeof(pthread_t) * threadCount);
	threadArgs = (param*)malloc(sizeof(param) * threadCount);
	BZRCConnect(IP_ADDRESS, ((argc == 2) ? 55555 : atoi(argv[2])), &tcpSocket, &fSocket);
	world.fSocket = fSocket;
	pthread_create(&worldThread, NULL, updater, (void*)&world);
	sem_wait(&mutex); // Wait for world data to be populated

	//pthread_create(&visualizerThread, NULL, visualizer, (void*)&world);
	//sem_wait(&mutex); // Wait for visualer data to be populated
	for (i = 0; i < threadCount; i++) {
		threadArgs[i].fSocket = fSocket;
		threadArgs[i].tankID = i;
		threadArgs[i].world = &world;
		threadArgs[i].flagGoalID = i % 3;
		pthread_create(&threads[i], NULL, driver, (void*)&threadArgs[i]);
	}

	for (i = 0; i < threadCount; i++) {
		pthread_join(threads[i], NULL);
	}

	pthread_join(worldThread, NULL);
	//pthread_join(visualizerThread, NULL);
	BZRCDisconnect(tcpSocket, fSocket);
	sem_destroy(&mutex);
	free(threadArgs);
	free(threads);
	printf("Agents shut down\n");
	return 0;
}

void setupFMatrix(gsl_matrix* matrix, double c) {
	//      _                          _
	//     | 1  dt  dt^2/2 0  0   0     |
	//     | 0  1   dt     0  0   0     |
	// F = | 0  -c  1      0  0   0     |
	//     | 0  0   0      1  dt  dt^2/2|
	//     | 0  0   0      0  1   dt    |
	//     |_0  0   0      0  -c  1    _|
	//
        gsl_matrix_set_zero(matrix);
	int i, j;
	for (i = 0, j = 0; i < 6, j < 6; i++, j++) 
		gsl_matrix_set(matrix, i, j, 1);
	gsl_matrix_set(matrix, 2, 1, -1 * c);
	//gsl_matrix_set(matrix, 1, 0, -1 * c);
	gsl_matrix_set(matrix, 5, 4, -1 * c);
	return;
}

void updateFMatrix(gsl_matrix* matrix, double deltaT) {
	//      _                          _
	//     | 1  dt  dt^2/2 0  0   0     |
	//     | 0  1   dt     0  0   0     |
	// F = | 0  -c  1      0  0   0     |
	//     | 0  0   0      1  dt  dt^2/2|
	//     | 0  0   0      0  1   dt    |
	//     |_0  0   0      0  -c  1    _|
	//
        gsl_matrix_set(matrix, 0, 1, deltaT);
        gsl_matrix_set(matrix, 0, 2, deltaT * deltaT / 2.0f);
        gsl_matrix_set(matrix, 1, 2, deltaT);
        gsl_matrix_set(matrix, 3, 4, deltaT);
        gsl_matrix_set(matrix, 3, 5, deltaT * deltaT / 2.0f);
        gsl_matrix_set(matrix, 4, 5, deltaT);
	return;
}

void setConstants(world_info* world) {
	int i;
	for (i = 0; i < world->constantCount; i++) {
		if (strcmp(world->constants[i].name, "contant") == 0) {
			// Set stuff here
		}
	}
	return;
}

void calcKalmanGainAtTPlus1(
        gsl_matrix* f,
	gsl_matrix* h,
	gsl_matrix* hTrans,
	gsl_matrix* sigma_t,
	gsl_matrix* sigma_x,
	gsl_matrix* sigma_z,
	gsl_matrix* k)
{
	// creating f transpose
	gsl_matrix* fTrans = gsl_matrix_calloc(6, 6);
	gsl_matrix_memcpy(fTrans, f);
	gsl_matrix_transpose(fTrans);
	//printf("F TRANS\n");printMatrix(fTrans);

	// storage matrix allocations	
	gsl_matrix* fstft = gsl_matrix_calloc(6, 6);
	gsl_matrix* m6x6 = gsl_matrix_calloc(6, 6);
	gsl_matrix* m6x2L = gsl_matrix_calloc(6, 2);

	gsl_matrix* m2x6R = gsl_matrix_calloc(2, 6);
	gsl_matrix* m2x2R = gsl_matrix_calloc(2, 2);
	gsl_matrix* m2x2RInv = gsl_matrix_calloc(2, 2);

	// left side to make a 6 x 2
	matrixMultiply(f, sigma_t, fstft);
	matrixMultiply(fstft, fTrans, m6x6);
	gsl_matrix_memcpy(fstft, m6x6);

	gsl_matrix_add(fstft, sigma_x);
	//printf("fstft\n");
	//printMatrix(fstft);

	matrixMultiply(fstft, hTrans, m6x2L);

	// right side to make 2 x 2
	matrixMultiply(h, fstft, m2x6R);
	matrixMultiply(m2x6R, hTrans, m2x2R);
	
	gsl_matrix_add(m2x2R, sigma_z);

	// invert
	invertMatrix2x2(m2x2R, m2x2RInv);
	
	matrixMultiply(m6x2L, m2x2RInv, k);
}

void calcMeanAtTPlus1(
	gsl_matrix* f,
	gsl_matrix* h,
        gsl_matrix* k,
	gsl_matrix* z,
	gsl_matrix* u,
	gsl_matrix* ut1)
{
	gsl_matrix* fu = gsl_matrix_calloc(6, 1);
	gsl_matrix* hf = gsl_matrix_calloc(2, 6);
	gsl_matrix* hfu = gsl_matrix_calloc(2, 1);
	matrixMultiply(f, u, fu);
	matrixMultiply(h, f, hf);
	matrixMultiply(hf, u, hfu);
	gsl_matrix_sub(z, hfu);
	matrixMultiply(k, z, ut1);
	gsl_matrix_add(ut1, fu);
}

void calcVarianceAtTPlus1(
	gsl_matrix* f,
	gsl_matrix* h,
	gsl_matrix* sigma_t,
	gsl_matrix* sigma_x,
        gsl_matrix* k,
	gsl_matrix* i,
	gsl_matrix* sigma_t1)
{
	// creating f transpose
	gsl_matrix* fTrans = gsl_matrix_calloc(6, 6);
	gsl_matrix_memcpy(fTrans, f);
	gsl_matrix_transpose(fTrans);

	// storage matrix allocations	
	gsl_matrix* fstft = gsl_matrix_calloc(6, 6);
	gsl_matrix* m6x6 = gsl_matrix_calloc(6, 6);

	gsl_matrix* kh = gsl_matrix_calloc(6, 6);
	gsl_matrix* lhs = gsl_matrix_calloc(6, 6);

	// calculate the left hand side
	gsl_matrix_memcpy(lhs, i);
	matrixMultiply(k, h, kh);
	gsl_matrix_sub(lhs, kh);

	//printf("lhs\n");
	//printMatrix(lhs);

	// calculate the right hand side
	matrixMultiply(f, sigma_t, fstft);
	matrixMultiply(fstft, fTrans, m6x6);
	gsl_matrix_memcpy(fstft, m6x6);
	gsl_matrix_add(fstft, sigma_x);

	//printf("rhs\n");
	//printMatrix(fstft);


	// get results
	matrixMultiply(lhs, fstft, sigma_t1);
}

void *driver(void* args) {
	param* params = (param*)args;
	FILE* fSocket = params->fSocket;
	world_info* world = params->world;
	char plotFileName[256];
	int tankID = params->tankID;
	int targetID = 0;

	// Potential Field stuff for shooting
	float previousAngle = 0;
	float speed = 0;
	pair totalDelta;

	// Kalman filter stuff
	float s = 50.0;
	pair futureEnemyLocation;
	float distanceToTarget = 0;
	int iterationsToHitTarget;
	int iterationsLeftToShoot;
	int converged = 0;
	//pthread_t gunnerThread;
	//pthread_create(&gunnerThread, NULL, gunner, args);
	enemyTank* enemyTanks;
	int enemyTankCount;
	int i = 0;
	struct timeval tvBegin, tvEnd, tvDiff;
	float deltaT;
	getOtherTanks(fSocket, &enemyTanks, &enemyTankCount);
	setSpeed(fSocket, tankID, 0);

	// Kalman Filter Matrix Initialization
	gsl_matrix* f = gsl_matrix_alloc(6, 6);
	gsl_matrix* h = gsl_matrix_alloc(2, 6);
	gsl_matrix* h_trans = gsl_matrix_alloc(6, 2);
	gsl_matrix* identity = gsl_matrix_alloc(6, 6);
	gsl_matrix* k = gsl_matrix_alloc(6, 2);
	gsl_matrix* u = gsl_matrix_calloc(6, 1); // U is initially all zeroes (thus calloc)
	gsl_matrix* ut1 = gsl_matrix_alloc(6, 1);
	gsl_matrix* z = gsl_matrix_alloc(2, 1); // Z is updated with our observation
	gsl_matrix* sigma_t = gsl_matrix_alloc(6, 6);
	gsl_matrix* sigma_t1 = gsl_matrix_calloc(6, 6);
	gsl_matrix* sigma_z = gsl_matrix_alloc(2, 2);
	gsl_matrix* sigma_x = gsl_matrix_alloc(6, 6);

	setupFMatrix(f, 0.1);
	updateFMatrix(f, 0.5);
	fillMatrix(h, H);
	fillMatrix(h_trans, H_TRANS);
	fillMatrix(identity, IDENTITY);
	fillMatrix(sigma_t, SIGMA_0);
	fillMatrix(sigma_x, SIGMA_X);
	fillMatrix(sigma_z, SIGMA_Z);

	// Sanity Check
	
	printf("F Matrix: \n");printMatrix(f);
	printf("H Matrix: \n");printMatrix(h);
	printf("I Matrix: \n");printMatrix(identity);
	printf("U Matrix: \n");printMatrix(u);
	printf("SIGMA_T Matrix: \n");printMatrix(sigma_t);
	printf("SIGMA_Z Matrix: \n");printMatrix(sigma_z);
	printf("SIGMA_X Matrix: \n");printMatrix(sigma_x);

/*	calcKalmanGainAtTPlus1(f, h, h_trans, sigma_t, sigma_x, sigma_z, k);
	printMatrix(k);
		gsl_matrix_set(z, 0, 0, 200);
		gsl_matrix_set(z, 1, 0, -10);
		gsl_matrix_set(u, 0, 0, -10);
		gsl_matrix_set(u, 1, 0, -10);
		gsl_matrix_set(u, 2, 0, -10);
		gsl_matrix_set(u, 3, 0, -10);
		gsl_matrix_set(u, 4, 0, -10);
		gsl_matrix_set(u, 5, 0, -10);
	calcMeanAtTPlus1(f, h, k, z, u, ut1);
	gsl_matrix_memcpy(u, ut1);
	printMatrix(ut1);
	calcStateAtTPlus1(f, h, sigma_t, sigma_x, k, identity, sigma_t1);
	printMatrix(sigma_t1);
*/

	while (running) {
	 
	  // Sleep
		free(enemyTanks);
		getOtherTanks(fSocket, &enemyTanks, &enemyTankCount);


		deltaT = (float)tvDiff.tv_usec;
		printf("slept for %2.5f seconds\n", deltaT/1000000);
		//updateFMatrix(f, deltaT/1000000); // uncomment this line for more accurate delta T in F matrix
		gsl_matrix_set(z, 0, 0, (double)enemyTanks[targetID].location.x);
		gsl_matrix_set(z, 1, 0, (double)enemyTanks[targetID].location.y);
		printf("Observation matrix:\n");printMatrix(z);

		// Kalman Filter
		calcKalmanGainAtTPlus1(f, h, h_trans, sigma_t, sigma_x, sigma_z, k);
		calcMeanAtTPlus1(f, h, k, z, u, ut1);
		calcVarianceAtTPlus1(f, h, sigma_t, sigma_x, k, identity, sigma_t1);

		gsl_matrix_memcpy(u, ut1);  // Update mean for next iteration
		gsl_matrix_memcpy(sigma_t,  sigma_t1);  // Update variance for next iteration

		if (!converged) {
		  sprintf(plotFileName, "plot%d.svg", i++);
		  plot(plotFileName, u, sigma_t);
			gettimeofday(&tvBegin, NULL);
			usleep(AGENT_SLEEP_TIME);
			gettimeofday(&tvEnd, NULL);
			timeval_subtract(&tvDiff, &tvEnd, &tvBegin);
			printf("Kalman Gain matrix:\n");printMatrix(k);
			printf("Mean matrix:\n");printMatrix(u);
			printf("Variance matrix:\n");printMatrix(sigma_t);
		}
		else {
			gettimeofday(&tvBegin, NULL);
			usleep(100000);
			gettimeofday(&tvEnd, NULL);
			timeval_subtract(&tvDiff, &tvEnd, &tvBegin);
			printf("Enemy Tank Location: (%2.1f,%2.1f)\n", enemyTanks[targetID].location.x, enemyTanks[targetID].location.y);
			printf("Enemy Tank Furure Location: (%2.1f,%2.1f)\n", futureEnemyLocation.x, futureEnemyLocation.y);
			printf("Distance to Target: %2.1f\n", distanceToTarget);
			printf("iterationsToHitTarget: %d\n", iterationsToHitTarget);
			printf("iterationsLeftToShoot: %d\n", iterationsLeftToShoot);
			enemyTanks[targetID].location.x = futureEnemyLocation.x;
			enemyTanks[targetID].location.y = futureEnemyLocation.y;
		}


		if (!converged && gsl_matrix_get(sigma_t, 0, 0) < 8.0) {
			converged = 1;
			futureEnemyLocation.x =  gsl_matrix_get(u, 0, 0) + 5 * gsl_matrix_get(u, 1, 0) * s;
			futureEnemyLocation.y =  gsl_matrix_get(u, 3, 0) + 5 * gsl_matrix_get(u, 4, 0) * s;
			distanceToTarget = getDistanceBetween(world->tanks[tankID].location.x, world->tanks[tankID].location.y,
							      futureEnemyLocation.x, futureEnemyLocation.y);
			iterationsToHitTarget = ((distanceToTarget / 100.0f) / 0.1f);
			iterationsLeftToShoot = s - iterationsToHitTarget - 10;
		}

		if (converged && iterationsLeftToShoot < 1) {
			shoot(fSocket, tankID);
			printf("Shooting dat sucka :D :D :D\n");
			fillMatrix(sigma_t, SIGMA_0);
			gsl_matrix_set_zero(u);
			converged = 0;
		}
		else {
			iterationsLeftToShoot--;
		}
		
			/*
		if (strcmp(enemyTanks[targetID].status, "alive") == 0 || distanceToTarget > 350) {
			fillMatrix(sigma_t, SIGMA_0);
			gsl_matrix_set_zero(u);
			converged = 0;
			targetID++;
			if (targetID > 1) {
				printf("Killed those suckas\n");
				running = 0;
				targetID = 0;
			}
		}
			*/

		totalDelta = seekGoal(
			world->tanks[tankID].location.x,
			world->tanks[tankID].location.y,
			enemyTanks[targetID].location.x,
			enemyTanks[targetID].location.y,
			1,
			&speed
		);
		previousAngle = move(
			fSocket,
			tankID,
			world->tanks[tankID].angle,
			previousAngle,
			totalDelta,
			speed,
			(float)tvDiff.tv_usec
		);
	}
	plot("myplot.gpi", u, sigma_t);
	setSpeed(fSocket, tankID, 0);
	setAngVel(fSocket, tankID, 0);
	//pthread_join(gunnerThread, NULL);

	free(enemyTanks);
	return 0;
}

void *gunner(void* args) {
	param* params = (param*)args;
	FILE* fSocket = params->fSocket;
	int tankID = params->tankID;
	int sleeptime;
	while (running) {
		shoot(fSocket, tankID);
		sleeptime = (rand() % 1000000) + 1500000;
		usleep(sleeptime);
	}
	return 0;
}


// Some shared memory problems and memory leak problems exist here
// They'd be easy to remove if they became a problem.  I'll do it later.
// world structures need to be freed when not in use by any tank, and
// world needs to be locked by updater thread when it performs updates
void *updater(void* args) {
	FILE* fSocket;
	world_info* world = (world_info*)args;
	fSocket = world->fSocket;
	//getFlags(fSocket, &world->flags, &world->flagCount);
	getMyTanks(fSocket, &world->tanks, &world->tankCount);
	//getBases(fSocket, &world->bases, &world->baseCount); // bases are static, don't update
	//getObstacles(fSocket, &world->obstacles, &world->obstacleCount);
	//getConstants(fSocket, &world->constants, &world->constantCount);
	sem_post(&mutex);
	while (running) {
		usleep(WORLD_UPDATE_RATE);
		//free(world->flags);
		//getFlags(fSocket, &world->flags, &world->flagCount);
		free(world->tanks);
		getMyTanks(fSocket, &world->tanks, &world->tankCount);
	}
	return 0;
}

/*void *visualizer(void* args) {
	FILE* fSocket;
	world_info* world = (world_info*)args;
	fSocket = world->fSocket;
	int argc = 0;
	char** argv = 0;
	int i;
	int j;
	setConstants(world);
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH);
	glutInitWindowSize(800, 800);
	glutInitWindowPosition(0, 0);
	glutCreateWindow("Grid Filter");
	grid = (float*)malloc(sizeof(float) * width * height);
	for (i = 0; i < height; i++) {
		for (j = 0; j < width; j++) {
			grid[i*width+j] = 0.75f;
		}
	}
	makeDestinations();
	//simulatePath();
	sem_post(&mutex);
	glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_CONTINUE_EXECUTION);
	glutDisplayFunc(drawGrid);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	signal(SIGINT, intHandler); // have to re-register these handlers since glut overwrote them
	signal(SIGKILL, intHandler);
	glutMainLoop();
	return 0;
}*/

pair seekGoal(float x, float y, float xg, float yg, float radiusg, float* retSpeed) {
	float distance = getDistanceBetween(x, y, xg, yg);
	float angle = getAngleBetween(x, y, xg, yg);
	float speed = 0;
	pair delta;
	delta.x = 0;
	delta.y = 0;
	if (distance < radiusg) {
		delta.x = 0;
		delta.y = 0;
		speed = 0;
	}
	else if (radiusg <= distance && distance <= (FLAG_SPREAD + radiusg)) {
		delta.x = FLAG_STEP * (distance - radiusg) * cos(angle);
		delta.y = FLAG_STEP * (distance - radiusg) * sin(angle);
		speed = sqrt(pow(delta.x,2) + pow(delta.y,2));
	}
	else if (distance > (FLAG_SPREAD + radiusg)) {
		delta.x = FLAG_STEP * FLAG_SPREAD * cos(angle);
		delta.y = FLAG_STEP * FLAG_SPREAD * sin(angle);
		speed = 1.0;
	}
	*retSpeed = speed;
	return delta;
}

float move(FILE* fSocket, int tankID, float currentAngle, float previousAngle, pair delta, float speed, float timeDiff) {
	float goalAngle = atan2(delta.y, delta.x);
	currentAngle = normalizeAngle(currentAngle);
	float errorAngle = goalAngle - currentAngle;
	float newAngle = errorAngle + ((errorAngle - previousAngle) / timeDiff);
	if (newAngle > PI) {
		newAngle = newAngle - 2 * PI;
	}
	else if (newAngle < -PI) {
		newAngle = 2 * PI + newAngle;
	}

	//setSpeed(fSocket, tankID, speed);
	setAngVel(fSocket, tankID, newAngle / PI);
	//printf("new angle: %f\n", newAngle);
	if (fabs(newAngle) < 0.01) {
		shoot(fSocket, tankID);
	}
	return newAngle;
}

float normalizeAngle(float angle) {
	float normalizedAngle;
	normalizedAngle = fmod(angle,(2 * PI));
	if (normalizedAngle >= PI) {
		normalizedAngle -= (2 * PI);
	}
	else if (normalizedAngle <= -PI) {
		normalizedAngle += (2 * PI);
	}
	return normalizedAngle;
}

float getDistanceBetween(float x, float y, float xg, float yg) {
	float distance;
	distance = sqrt(pow((x - xg),2) + pow((y - yg),2));
	return distance;
}

float getAngleBetween(float x, float y, float xg, float yg) {
	float angle;
	angle = atan2((yg - y),(xg - x));
	return angle;
}

void intHandler() {
	printf("Shutting down agents...\n");
	running = 0;
	return;
}

int timeval_subtract(struct timeval *result, struct timeval *t2, struct timeval *t1) {
    long int diff = (t2->tv_usec + 1000000 * t2->tv_sec) - (t1->tv_usec + 1000000 * t1->tv_sec);
    result->tv_sec = diff / 1000000;
    result->tv_usec = diff % 1000000;

    return (diff<0);
}

void plot(char* filename, gsl_matrix* u, gsl_matrix* sigma) {
	FILE* outputFile;
	char* gnuplotArgs[4];
	char plotDataFileName[256];
	char gnuplotLocation[] = "/usr/bin/gnuplot";
	char gnuplotFile[] = "myplot.gpi";
	char gnuplotArg[] = "-persist";
	sprintf(plotDataFileName, "%s.gpi", filename);
	outputFile = fopen(plotDataFileName, "w");
	fprintf(outputFile, "set xrange [-400.0: 400.0]\n");
	fprintf(outputFile, "set yrange [-400.0: 400.0]\n");
	fprintf(outputFile, "set pm3d\n");
	fprintf(outputFile, "set view map\n");
	
	fprintf(outputFile, "set term svg\n");
	fprintf(outputFile, "set output '%s'\n", filename);

	fprintf(outputFile, "unset key\n");
	fprintf(outputFile, "set size square\n");
	fprintf(outputFile, "set palette model RGB functions 1-gray, 1-gray, 1-gray\n");
	fprintf(outputFile, "set isosamples 100\n");
	fprintf(outputFile, "sigma_x = %f\n", gsl_matrix_get(sigma, 0, 0));
	fprintf(outputFile, "sigma_y = %f\n", gsl_matrix_get(sigma, 3, 3));
	fprintf(outputFile, "mean_x = %f\n", gsl_matrix_get(u, 0, 0));
	fprintf(outputFile, "mean_y = %f\n", gsl_matrix_get(u, 3, 0));
	fprintf(outputFile, "splot (1.0/(2.0 * pi * sigma_x * sigma_y) * exp(-1.0/2.0 * ((x - mean_x)**2 / sigma_x**2 + (y - mean_y)**2 / sigma_y**2))) with pm3d\n");
	fclose(outputFile);
	if (fork() == 0) {
		gnuplotArgs[0] = gnuplotLocation;
		gnuplotArgs[1] = plotDataFileName;
		gnuplotArgs[2] = gnuplotArg;
		gnuplotArgs[3] = 0;
		execv(gnuplotLocation, gnuplotArgs); 
	}
	return;
}

